import csv
import ast  # 用于把字符串解析成 Python 对象
import numpy as np
import matplotlib.pyplot as plt
vel_body = []
ankle_torques = []


with open("../data/data_continus_walking_1dot25_extras_log.csv", "r") as f:
    reader = csv.reader(f)
    for row in reader:
        # 假设 vel_body 在第 10 列（索引 9）
        vel_body_str = row[9]  # 取出字符串
        ankle_torques_str = row[1]
        # 将字符串转换回 Python 列表
        vel_body_list = ast.literal_eval(vel_body_str)
        ankle_torques_list = ast.literal_eval(ankle_torques_str)
        vel_body.append(vel_body_list)
        ankle_torques.append(ankle_torques_list)


vel_body = np.array(vel_body)
ankle_torques = np.array(ankle_torques)
print("vel_body.shape: ", vel_body.shape)  # 例如 (num_rows, 32)
print("ankle_torques.shape", ankle_torques.shape)
# vel_body.shape:  (1839, 256)
# ankle_torques.shape (1839, 256, 2)


save_path = "flattened_data.csv"

with open(save_path, "w", newline="") as f:
    writer = csv.writer(f)

    # ---- 写表头 ----
    header = ["serial"]

    for i in range(256):
        header += [f"vel_{i}"]
        header += [f"left_{i}"]
        header += [f"right_{i}"]
    writer.writerow(header)

    # ---- 写每一行 ----
    for i in range(len(vel_body)):
        row = []
        row.append(i)
        for j in range(256):
            row.append(vel_body[i, j])
            row.append(ankle_torques[i, j, 0])
            row.append(ankle_torques[i, j, 1])
        writer.writerow(row)


# 画图
plt.figure(figsize=(10, 5))

# 画线
plt.plot(vel_body[200:300,1], label="Vel Body Torques")
plt.plot(ankle_torques[200:300,1,0], label="Ankle Torques")




# 画点（使用相同的数据）
plt.scatter(range(len(vel_body[200:300,1])), vel_body[200:300,1])
plt.scatter(range(len(ankle_torques[200:300,1,0])), ankle_torques[200:300,1,0])

plt.xlabel("Time Step")
plt.ylabel("Velocity (m/s)")
plt.title("vel_body")
plt.legend()
plt.grid(True)
plt.show()


